/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: globaldefs.h 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <pthread.h>
#include <stdbool.h>
#include <sys/netmgr.h>
#include <sys/neutrino.h>

#if !defined( GLOBALDEFS_H )
#define GLOBALDEFS_H  

#ifndef TRUE 
#define TRUE			1
#endif
#ifndef FALSE
#define FALSE		       -1
#endif
#define	byte			unsigned char
#define word			unsigned short
#define NSECS_PER_SEC		1000000000
#define CHECKSUM_ERROR		1
#define D2R				0.0174532925199432957692369076848
#define pi			    3.1415926535897932384626433832795
#define R2D				57.295779513082320876798154814105
#define dT				0.001
#define MAX_WAYPOINTS	50

#define fifoAttiDatalog "/flash/fifo0"
#define fifoNavDatalog "/flash/fifo1"
#define fifoAttiTele "/flash/fifo2"
#define fifoNavTele "/flash/fifo3"


//Uncomment to compile for Linux
//#define LINUX 

//Uncomment to compile for QNX
//#define QNX

//Uncomment to compile for Win32
//#define WIN_OFFICE

//Uncomment to compile for Win32
#define WIN_HOME

//Uncoment to save
#define SAVE TRUE

//Uncomment for grphics
#define NCURSE_DISPLAY_OPTION 

//Uncemment for debuging
#define DEBUG 

////////////////////////////////
//Prueba Timer
#define MY_PULSE_CODE		_PULSE_CODE_MINAVAIL


typedef union {
struct _pulse   pulse;
	/* your other message structures would go here too */
} my_message_t;
////////////////////////////////

struct imu{
	float	p,q,r;						/* angular velocities	*/
	float	ax,ay,az;				/* acceleration			*/
	float	dvx,dvy,dvz;				/* delta v's			*/
	float	dthetax,dthetay,dthetaz;	/* delta theta's		*/
	float  roll,pitch,yaw;
	float	T11,T12,T13;				/* DCM components		*/
	float	T21,T22,T23;
	float	T31,T32,T33;
	float  time;
	unsigned long	timer_imu;
	short int error_type;
};

struct imuShort{
	float roll;
	float Refroll;
	float uroll;
	float pitch;
	float Refpitch;
	float upitch;
	float yaw;
	float Refyaw;
	float uyaw;
	int	  Ts;
	long int counter;
};

struct gps {
   float lat,lon,alt_msl;          /* gps position          */
   float  ve,vn,vd;             /* gps velocity          */
   word   ITOW;
   short  err_type;             /* error type            */
   float time;
};

struct adc{
	float  airSpeed;
	float  pAltitude;
};

struct gpsAdc{
	float latRef;
	float lonRef;
	float lat,lon,alt_msl;          /* gps position          */
	float  ve,vn,vd;             /* gps velocity          */
	float  airSpeed;
	float  RefairSpeed;
	float  uairSpeed;
	float  pAltitude;
	float  RefpAltitude;
	float  upAltitude;
	int	  Ts;
	long int counter;
};
/** 
 * A structure that contains all of the information necessary for a PID controller based on Tuetin discretization
 */

struct PIDCtrl{
	struct {
		float R;			// Input: Set point
		float y;			// Input: Measured Variable
		float u;			// Output: Controller Output


	} Signals;
	struct {
		float R1;		// Filtered Reference
		float R2;		// Filtered Reference
		float R3;		// Filtered Reference
		float e;		// Actual Error
		float e_1;		// Error E(k-1)
		float e_2;		// Error E(k-2)
		float u_1;		// Control Action u(k-1)
		float I;		// Integration Signal
		float D;		// Derivation Signal
	} States;
	struct {
		float K;		// Controller gain
		float Ti;		// Integral time
		float Td;		// Derivative time
		float ulow;		// Low output limit
		float uhigh;	// High output limit
		float Ts;		// Sampling Period
		float u0;		// Operation Point Control Action

	} Par;
};

struct nav {
   float lat,lon,alt;
   float ve,vn,vd;
// float  t;
   short  err_type;
   float time;
};

struct servos {
   unsigned short int roll;
   unsigned short int pitch;
   unsigned short int yaw;
   unsigned short int thrust;
};

struct dms{
		double hours;
		double minutes;
		double seconds;
};

struct telemetry{
	float lat,lon,alt;
	float ve,vn,vd;
	float roll,pitch,yaw;
	float airSpeed;
	float pAltitude;
	
};

struct wayPoints{
	int numPoint;
	float latRef;
	float lonRef;
	float altRef;
	float velRef;
};

struct rotMat{
	float vn;
	float ve;
	float vd;
	float Heading;
};
//struct  imu   imupacket;
//struct  gps   gpspacket;
struct  nav   navpacket;

// Visualization Variable
#define v 1
//Time Precision
#define prec 1000

//----------------------------------------------- 
//Threads
//----------------------------------------------- 
pthread_t ID_stateMachine,ID_attitudeCtrl,ID_navigationCtrl,ID_cameraCtrl;
pthread_t ID_idle,ID_healthMntr,ID_telemetry,ID_datalogging,ID_watchdog;
pthread_attr_t attr_stateMachine, attr_attitudeCtrl, attr_navigationCtrl, attr_cameraCtrl;
pthread_attr_t attr_idle, attr_healthMntr, attr_telemetry, attr_datalogging, attr_watchdog;
struct sched_param param;

//----------------------------------------------- 
// Global Variables without Mutex
//----------------------------------------------- 
/*	int periodos_perdidos=0;
	int fd_fifo;
	struct timespec TS; */
	struct timespec Ts_idle,Ts_attitudeCtrl,Ts_navigationCtrl,Ts_cameraCtrl,Ts_telemetry;
	struct timespec Ts_healthMntr,Ts_datalogging,Ts_watchdog,Ts_stateMachine;
//----------------------------------------------- 
// Global Variables with Mutex
//-----------------------------------------------

//----------------------------------------------- 
// PID Declaration
//----------------------------------------------- 
struct PIDCtrl pidRoll;
struct PIDCtrl pidPitch;
struct PIDCtrl pidYaw;
//----------------------------------------------
struct PIDCtrl pidVelocity;
struct PIDCtrl pidAltitude;
struct PIDCtrl pidHeading;
//----------------------------------------------- 
// Mutex
//-----------------------------------------------
static pthread_mutex_t	mutexPidLowLevel = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t	mutexPidHighLevel = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t	mutexGlobalVar = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t	mutexMachineState = PTHREAD_MUTEX_INITIALIZER;

//----------------------------------------- 
struct rotMat rotMatYawHeading;
int semiAutomatic;
struct wayPoints flightPlan[MAX_WAYPOINTS];
int actualWayPoint;
int totalWayPoints;
int event_0,event_1,event_2,event_3,event_4,event_5;
int X1_idle, X2_takeoff, X3_navigation, X4_landing;
#endif
//mutex and conditional variables
//
//pthread_mutex_t	mutex_gps;
//pthread_mutex_t mutex_nav;
//pthread_cond_t  trigger_ahrs;
//pthread_cond_t  trigger_nav;

